#include <check.h>
#include <libswiftnav/correlate.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define L1CA_CHIPS_PER_PRN_CODE   1023
#define L2C_CM_CHIPS_PER_PRN_CODE 10230

enum signal_type {
  L1CA_SIGNAL,
  L2C_SIGNAL
};

//#define DUMP_RESULTS

#ifdef DUMP_RESULTS
#include <time.h>

static clock_t start = 0;
static clock_t stop = 0;

static void debug_start_timing(void)
{
  start = clock();
}

static void debug_stop_timing(void)
{
  stop = clock();
}

static void debug_dump_results(u32 num_samples,
                               double I_E, double Q_E,
                               double I_P, double Q_P,
                               double I_L, double Q_L)
{
  printf("==============================================\n");
  printf("used %f cycles\n", (double)stop - start);

  printf("num_samples = %d\n", num_samples);

  printf("\n");

  printf("I_E / I_P = %f\n", (0 == I_P) ? 0 : I_E / I_P);
  printf("I_L / I_P = %f\n", (0 == I_P) ? 0 : I_L / I_P);

  printf("\n");

  printf("I_E = %f\n", I_E);
  printf("Q_E = %f\n", Q_E);
  printf("Q_E / I_E = %f\n", (0 == I_E) ? 0 : Q_E / I_E);

  printf("\n");

  printf("I_P = %f\n", I_P);
  printf("Q_P = %f\n", Q_P);
  printf("Q_P / I_P = %f\n", (0 == I_P) ? 0 : Q_P / I_P);

  printf("\n");

  printf("I_L = %f\n", I_L);
  printf("Q_L = %f\n", Q_L);
  printf("Q_L / I_L = %f\n", (0 == I_L) ? 0 : Q_L / I_L);

  printf("\n");
}
#else
#define debug_dump_results(num_samples, I_E, Q_E, I_P, Q_P, I_L, Q_L)
#define debug_start_timing()
#define debug_stop_timing()
#endif

/* PRN 1. One bit per sample. */
static const u8 gps_l1ca_code[] = {
  0xC8, 0x39, 0x49, 0xE5, 0x13, 0xEA, 0xD1, 0x15, 0x59, 0x1E, 0x9F,
  0xB7, 0x37, 0xCA, 0xA1, 0x00, 0xEA, 0x44, 0xDE, 0x0F, 0x5C, 0xCF,
  0x60, 0x2F, 0x3E, 0xA6, 0x2D, 0xC6, 0xF5, 0x15, 0x82, 0x01, 0x03,
  0x1D, 0x81, 0xC6, 0xFF, 0xA7, 0x4B, 0x61, 0x56, 0x27, 0x2D, 0xD8,
  0xEE, 0xF0, 0xD8, 0x64, 0x90, 0x6D, 0x2D, 0xE2, 0xE0, 0x52, 0x7E,
  0x0A, 0xB9, 0xF5, 0xF3, 0x31, 0xC6, 0xD5, 0x6C, 0x6E, 0xE0, 0x02,
  0xCD, 0x9D, 0xA0, 0xAB, 0xAE, 0x94, 0x73, 0x89, 0x45, 0x2D, 0x0A,
  0xDA, 0xD8, 0xE7, 0xB2, 0x1F, 0x96, 0x88, 0x7D, 0x5C, 0xC9, 0x25,
  0xFF, 0x87, 0xDE, 0x37, 0x2C, 0x39, 0x50, 0xA5, 0x7E, 0x3D, 0xA7,
  0x67, 0xEF, 0xA3, 0x1F, 0x01, 0x28, 0xB4, 0x44, 0xD8, 0x1D, 0xA3,
  0x44, 0x8E, 0x2C, 0xC9, 0xE6, 0xFC, 0xCA, 0x69, 0xAF, 0x36, 0xA7,
  0x78, 0xD4, 0x42, 0x24, 0xE1, 0xCA, 0x20
};

static const u8 gps_l2cm_code[] = {
  0x2B, 0xDE, 0x1E, 0xBA, 0x29, 0x87, 0xCB, 0x12, 0xA8, 0xFA, 0xF0,
  0xFA, 0xBA, 0xD8, 0x1E, 0xCC, 0x07, 0xB3, 0xF1, 0x84, 0xCB, 0xDA,
  0x5D, 0xE0, 0x48, 0x62, 0x87, 0xCF, 0xC9, 0x69, 0xE0, 0x39, 0x5C,
  0x77, 0x26, 0xB1, 0x4B, 0x36, 0xF6, 0xA9, 0xBC, 0x16, 0x85, 0x16,
  0x39, 0x3A, 0x5D, 0x7B, 0xD6, 0x28, 0xBD, 0x8F, 0xDF, 0x9A, 0xE9,
  0x10, 0x2E, 0xAD, 0xD0, 0xCA, 0x1C, 0xE4, 0xBE, 0x05, 0x97, 0x38,
  0xB4, 0x71, 0xC0, 0xB2, 0xEC, 0x7F, 0x4B, 0x34, 0xA4, 0xA3, 0xF6,
  0x7A, 0x3C, 0x5B, 0x3A, 0xAA, 0x87, 0x98, 0x53, 0xF0, 0xB3, 0x9A,
  0xC5, 0xE6, 0xD9, 0x7F, 0x26, 0x1D, 0x71, 0xB7, 0x75, 0xA8, 0x0C,
  0x77, 0x78, 0xCF, 0x3C, 0xC6, 0xF4, 0x4E, 0x20, 0x62, 0x8B, 0x2B,
  0x2B, 0x4E, 0xBE, 0xF3, 0x67, 0xD1, 0x65, 0x49, 0x11, 0x83, 0xB9,
  0x21, 0xBA, 0x38, 0xB5, 0x21, 0x3D, 0x5E, 0xC1, 0x7D, 0xF8, 0x43,
  0x58, 0x37, 0x45, 0xE8, 0x27, 0xAD, 0x99, 0x46, 0xC6, 0x96, 0xB8,
  0xE6, 0x3B, 0x51, 0xC2, 0x36, 0xD8, 0xA3, 0x9D, 0x39, 0x0A, 0x65,
  0x1E, 0xEA, 0xEC, 0x8E, 0x0A, 0xF3, 0xC1, 0x0A, 0xA7, 0xEB, 0x80,
  0x32, 0x82, 0x3D, 0x5B, 0x35, 0xDA, 0xCA, 0xFA, 0x94, 0x83, 0x9A,
  0x96, 0x6A, 0xB0, 0xD8, 0xD7, 0x69, 0x7C, 0x73, 0xDF, 0xC2, 0x54,
  0x25, 0x42, 0x4B, 0xA2, 0xED, 0x1B, 0x5E, 0xC7, 0xB4, 0x76, 0xA7,
  0x1D, 0x9B, 0x4D, 0x03, 0xCB, 0xDA, 0x81, 0xAD, 0xF3, 0x60, 0x76,
  0x23, 0xC6, 0x24, 0xC1, 0x90, 0xB3, 0x3C, 0xAA, 0x56, 0xED, 0xD3,
  0x25, 0x17, 0xD6, 0x3E, 0x6F, 0xA0, 0x7B, 0x52, 0x9E, 0xAE, 0xE9,
  0xD3, 0x26, 0xE3, 0x64, 0xE8, 0x9E, 0xFE, 0xD1, 0xF6, 0x52, 0xBE,
  0x5A, 0xB2, 0x60, 0xDC, 0xEF, 0x30, 0x16, 0x1E, 0x89, 0xD6, 0xBD,
  0xB0, 0xE6, 0x68, 0xC0, 0x5F, 0x94, 0x71, 0xBC, 0x0F, 0xD8, 0xB6,
  0x8C, 0x7F, 0x93, 0x90, 0x96, 0x97, 0xA2, 0x16, 0xA6, 0x2E, 0x86,
  0xD5, 0x4D, 0xF3, 0x65, 0xDC, 0x7E, 0x3E, 0x7E, 0xC3, 0xA8, 0x9C,
  0x11, 0x9C, 0x51, 0x0D, 0x8A, 0xE8, 0x6C, 0x35, 0xF3, 0x53, 0x91,
  0x30, 0x35, 0x65, 0x51, 0x99, 0x7D, 0xFF, 0x95, 0x31, 0x34, 0xCC,
  0x4B, 0x96, 0x54, 0x1A, 0xA7, 0x4F, 0x81, 0x1D, 0xF7, 0x1B, 0x2C,
  0xAB, 0xFD, 0xBB, 0x9C, 0xC3, 0x6D, 0x3B, 0xE1, 0x18, 0x55, 0x4E,
  0x62, 0x6D, 0xE9, 0xF2, 0xB1, 0x34, 0x3E, 0x65, 0x44, 0x37, 0x20,
  0x35, 0x51, 0xB9, 0x86, 0x40, 0x28, 0x77, 0x21, 0x6F, 0x4E, 0xC1,
  0x65, 0x12, 0xF5, 0xFA, 0xA0, 0x13, 0xFF, 0xFA, 0x2D, 0x59, 0xDE,
  0x4C, 0xC5, 0xD0, 0x02, 0xF5, 0x5A, 0x45, 0x98, 0x51, 0x15, 0x80,
  0x3D, 0xCF, 0xFB, 0x3E, 0x04, 0x48, 0x02, 0x1C, 0x0F, 0x74, 0xB4,
  0xAE, 0x8D, 0x79, 0xE1, 0x52, 0xE3, 0xC0, 0xC7, 0xC1, 0x70, 0x05,
  0x25, 0x95, 0xE3, 0xB6, 0xB5, 0x33, 0x98, 0x50, 0xD0, 0x49, 0x6C,
  0x47, 0x33, 0xB4, 0xB9, 0x73, 0x12, 0x79, 0xDA, 0x8A, 0x4F, 0x9A,
  0x8A, 0x44, 0xB4, 0xCC, 0x4C, 0x0D, 0x3C, 0x2F, 0xC9, 0x67, 0x04,
  0x84, 0x8E, 0x41, 0x94, 0x27, 0x26, 0xF7, 0x3B, 0xEC, 0x95, 0x25,
  0x26, 0x3C, 0x8D, 0x5A, 0x33, 0x0C, 0x3F, 0x65, 0x51, 0x1C, 0xB7,
  0x8F, 0x22, 0x14, 0xB3, 0xFA, 0x10, 0x6C, 0x29, 0x1E, 0xF5, 0x06,
  0xF7, 0x4D, 0x68, 0x22, 0xBD, 0xC3, 0x2C, 0x05, 0x05, 0x62, 0xD4,
  0x5B, 0x32, 0xF7, 0x19, 0xBA, 0x17, 0x5A, 0xB6, 0x41, 0xF7, 0xF0,
  0xAE, 0x5A, 0xEB, 0x9F, 0x98, 0xF0, 0x84, 0x6D, 0xD9, 0x78, 0x17,
  0x4D, 0x57, 0x17, 0x89, 0xF0, 0xEF, 0xD2, 0x66, 0x4E, 0x22, 0xD1,
  0xDB, 0x49, 0x28, 0x9E, 0xC5, 0x4A, 0xC8, 0x53, 0xD3, 0x03, 0x5E,
  0x23, 0xB6, 0x57, 0x1F, 0xB5, 0x98, 0xA9, 0x1E, 0x1C, 0x23, 0xB9,
  0xAB, 0x76, 0xCB, 0x79, 0xB2, 0x4B, 0x05, 0x6B, 0x2E, 0xD8, 0x31,
  0x83, 0xDF, 0x15, 0x8A, 0xFF, 0x03, 0x12, 0xE6, 0x3E, 0x82, 0x67,
  0x07, 0x99, 0xFF, 0xB0, 0xE0, 0x13, 0xAA, 0xDB, 0x5A, 0x42, 0xB2,
  0xD8, 0x71, 0x73, 0x20, 0x4A, 0x32, 0x37, 0x52, 0xC4, 0x29, 0x52,
  0x69, 0x2E, 0xCC, 0xA6, 0xD0, 0x06, 0x05, 0xAA, 0xBD, 0xD5, 0x11,
  0x16, 0x1F, 0xF2, 0x86, 0x27, 0x82, 0x7B, 0x2E, 0x89, 0x85, 0x16,
  0xFC, 0x67, 0x47, 0xFA, 0x85, 0x81, 0x6D, 0xE2, 0xBB, 0x97, 0xAE,
  0xB9, 0x68, 0x78, 0x34, 0x1E, 0x00, 0x3A, 0x66, 0x54, 0x21, 0x0E,
  0x50, 0xC3, 0x77, 0xEA, 0x7C, 0xD1, 0x81, 0xCE, 0xDD, 0x6E, 0x80,
  0xD9, 0x97, 0xC4, 0xE8, 0x94, 0x4C, 0xB0, 0x0B, 0xBD, 0x30, 0x0B,
  0x8A, 0x64, 0xE6, 0x4E, 0xD3, 0x93, 0x57, 0x57, 0xDA, 0xD9, 0xCF,
  0x35, 0x37, 0x39, 0xAD, 0x7E, 0xC8, 0x94, 0x18, 0xCC, 0x17, 0x16,
  0xA0, 0x60, 0x97, 0x36, 0xF4, 0x40, 0x49, 0xAB, 0x03, 0x66, 0x61,
  0x01, 0x00, 0x2A, 0x00, 0x1C, 0xF7, 0xB3, 0x07, 0x52, 0x70, 0x4E,
  0xC0, 0x31, 0x45, 0xF7, 0x41, 0xD7, 0xE7, 0xDD, 0x93, 0x13, 0x1E,
  0x17, 0xC9, 0x20, 0x21, 0xC5, 0x3D, 0xB0, 0x90, 0x67, 0x95, 0x6B,
  0xBC, 0xB4, 0xDA, 0x04, 0xBC, 0x61, 0xB3, 0xD1, 0x8A, 0x5D, 0x49,
  0xD6, 0xD8, 0x6A, 0xA3, 0x37, 0xC4, 0xFF, 0x00, 0xF6, 0xC3, 0x4A,
  0x0A, 0xB4, 0xC0, 0xB9, 0xDD, 0x04, 0xA5, 0x05, 0x7C, 0xC7, 0xE2,
  0xBB, 0x99, 0x86, 0x65, 0x12, 0x88, 0x5D, 0x7C, 0x6E, 0xCF, 0x40,
  0xA9, 0xCC, 0x8F, 0xF4, 0x4A, 0xEA, 0x0D, 0x1A, 0xAD, 0xF2, 0x98,
  0x7A, 0x3F, 0xF5, 0xB1, 0x37, 0x68, 0x1F, 0xE1, 0x89, 0xCB, 0x81,
  0xC5, 0x49, 0xEE, 0x56, 0xBC, 0x5C, 0xC4, 0xAA, 0x8F, 0x5D, 0x6F,
  0x33, 0x83, 0x07, 0xE3, 0x71, 0xE0, 0xC8, 0xC6, 0xA7, 0xDC, 0x6A,
  0x2F, 0xE2, 0x5B, 0x19, 0x33, 0xD2, 0x10, 0x3C, 0xB9, 0x56, 0xE5,
  0xDE, 0x27, 0x69, 0x46, 0x16, 0x12, 0x46, 0xD7, 0xA6, 0xBC, 0x0F,
  0x3B, 0xF7, 0x99, 0x16, 0x7C, 0x98, 0x29, 0xC1, 0x43, 0x50, 0x9C,
  0x4C, 0x64, 0x05, 0x27, 0x6E, 0xBD, 0xB1, 0x6A, 0x96, 0xDD, 0x3D,
  0xDB, 0xB3, 0x6B, 0x1D, 0x01, 0xC5, 0x29, 0x15, 0xAC, 0xB7, 0xCF,
  0xB3, 0xE8, 0x73, 0x3A, 0x13, 0xBD, 0x98, 0xAD, 0x20, 0x96, 0x4E,
  0x34, 0xB6, 0xBF, 0x52, 0xEB, 0xA9, 0x3C, 0x88, 0x53, 0x03, 0x3A,
  0x87, 0x59, 0x3A, 0xE2, 0x6D, 0xA8, 0xF4, 0xD1, 0x1A, 0x72, 0x6F,
  0xFF, 0xF3, 0xBA, 0x1F, 0x58, 0xCC, 0xA5, 0x40, 0xB8, 0x1F, 0x18,
  0xD6, 0xF5, 0x66, 0xF8, 0x75, 0xBA, 0x9C, 0xD0, 0xD0, 0x5A, 0xED,
  0x46, 0xCF, 0xFC, 0xF5, 0xF0, 0x21, 0xB8, 0xB6, 0xF2, 0xD0, 0x53,
  0x16, 0x60, 0x71, 0x25, 0xB8, 0x12, 0x5D, 0xD6, 0x6A, 0xD5, 0x11,
  0x00, 0xA5, 0x17, 0xF4, 0x20, 0xAD, 0xF0, 0x7A, 0xE6, 0x47, 0x7F,
  0x73, 0x91, 0x2A, 0xD1, 0xEA, 0x90, 0x51, 0x1F, 0xB1, 0x4E, 0xFE,
  0x58, 0x04, 0x78, 0x6E, 0xD3, 0x15, 0x69, 0x09, 0x2F, 0x96, 0x6F,
  0x92, 0x97, 0x49, 0x18, 0x1A, 0x4B, 0xEF, 0xDB, 0xD1, 0x77, 0xA6,
  0xFF, 0x31, 0xA7, 0x5D, 0x56, 0xA5, 0xCF, 0x1E, 0x8A, 0x0A, 0xCE,
  0x4E, 0x75, 0x61, 0x4A, 0xD1, 0x39, 0x13, 0xEC, 0x6A, 0xEB, 0x41,
  0xB2, 0x1D, 0x8E, 0xA1, 0xD2, 0x56, 0x2E, 0xF9, 0x0F, 0x9C, 0x5C,
  0xB0, 0x35, 0x05, 0xC3, 0x0C, 0x1D, 0x0E, 0x31, 0x0B, 0xBA, 0x71,
  0x6A, 0x2C, 0x68, 0xD7, 0xB6, 0x32, 0x6F, 0x3D, 0x2F, 0x93, 0x71,
  0xBD, 0xF9, 0x2E, 0xA8, 0x13, 0xDF, 0xAA, 0xFA, 0xB1, 0x0B, 0x1D,
  0x1E, 0x69, 0x76, 0xEB, 0x91, 0xD1, 0x2F, 0x61, 0xE5, 0x7B, 0x9A,
  0x1E, 0xC4, 0xC8, 0x91, 0x94, 0xC3, 0xAA, 0xE9, 0xA2, 0x64, 0x78,
  0x83, 0x2B, 0x65, 0x0C, 0x03, 0x2F, 0x7B, 0x1F, 0x05, 0x6F, 0x98,
  0x67, 0x1F, 0x03, 0x78, 0x92, 0xDB, 0x88, 0x1D, 0x04, 0x83, 0xF6,
  0xD3, 0x05, 0x9D, 0x46, 0x0F, 0x95, 0x54, 0x40, 0x3C, 0xCC, 0xEC,
  0x8B, 0x5A, 0xD8, 0xA8, 0x9C, 0x36, 0xD0, 0x18, 0xB8, 0xA6, 0xEC,
  0xC5, 0xF1, 0x1D, 0xE2, 0xB8, 0x35, 0xE3, 0x3E, 0x0B, 0x92, 0x39,
  0x27, 0x4B, 0x6F, 0xDD, 0x77, 0x5D, 0x07, 0xB0, 0xB0, 0x5A, 0x87,
  0x06, 0x9D, 0x43, 0x3E, 0x9A, 0x87, 0x88, 0xB8, 0x39, 0xC4, 0xAC,
  0x71, 0x46, 0x2A, 0xF7, 0x32, 0xFB, 0x8A, 0x14, 0xB1, 0xFE, 0xAB,
  0x3D, 0x9A, 0xAF, 0x82, 0x37, 0x43, 0xBE, 0xEA, 0x3D, 0x65, 0xBE,
  0x31, 0xA7, 0x5C, 0x48, 0xA9, 0xDF, 0x32, 0xB2, 0x63, 0x92, 0x49,
  0xE6, 0xED, 0xBE, 0xE6, 0x04, 0xD4, 0x2E, 0x7D, 0xB0, 0xB3, 0x88,
  0xFB, 0x01, 0x52, 0x64, 0x93, 0xA1, 0x95, 0xA9, 0xFA, 0x4D, 0x07,
  0xF8, 0x5A, 0x24, 0x6A, 0x6A, 0x88, 0x36, 0x3B, 0x20, 0xC1, 0x24,
  0x59, 0x46, 0x3C, 0xDD, 0x57, 0xD6, 0xAE, 0xA8, 0x7C, 0x5C, 0xCA,
  0xD6, 0x42, 0xB0, 0xFB, 0x73, 0x90, 0xD6, 0xCC, 0xA9, 0x88, 0x16,
  0xDB, 0x94, 0xE0, 0xE7, 0x68, 0x9E, 0xE3, 0xC3, 0x9A, 0x69, 0xA1,
  0x0A, 0xDB, 0xAA, 0x86, 0x42, 0xE9, 0x0D, 0xFE, 0xC6, 0xD5, 0x24,
  0xDB, 0x8D, 0xBA, 0xA6, 0x48, 0x16, 0x4E, 0xF4, 0x4C, 0xD5, 0xC6,
  0x8E, 0x58, 0xBB, 0xC4, 0x41, 0x73, 0xDC, 0x4A, 0xAA, 0xD8, 0x2E,
  0x12, 0x62, 0x83, 0x2A, 0x1A, 0xD1, 0x52, 0xAD, 0x4D, 0x50, 0xCE,
  0xF1, 0x06, 0x28
};

struct signal {
  s8* samples;
  size_t size;
};

struct signal generate_signal(enum signal_type signal_type,
                              double if_freq, double code_freq,
                              double carr_doppler_freq,
                              double carr_to_code, double sampling_freq,
                              s8* prn_code, u32 ms_to_generate)
{
  u32 i;
  double delt;
  double code_phase = 0;
  double carr_phase = 0;
  u32 samples_num;
  double carr_freq;
  double carr_phasestep;
  double code_phasestep;
  s8* samples;
  struct signal signal = {NULL, 0};

  samples_num = (u32)(sampling_freq * 1e-3 * ms_to_generate);
  samples = malloc(samples_num);
  if (NULL == samples) {
    return signal;
  }
  delt = 1. / sampling_freq;
  carr_freq = if_freq + carr_doppler_freq;
  code_freq += carr_doppler_freq * carr_to_code;
  carr_phasestep = carr_freq * delt;
  code_phasestep = code_freq * delt;

  for (i = 0; i < samples_num; i++) {
    s8 code = 0;
    double ip;
    switch (signal_type) {
    case L1CA_SIGNAL:
      code = prn_code[(int)code_phase];
      break;
    case L2C_SIGNAL:
      if ((int)code_phase & 1) {
        code = 0; // hit CL
      } else {
        code = prn_code[(int)code_phase / 2];
      }
      break;
    }

    static bool header = 1;
    static FILE *f;
    if (header) {
       f = fopen("./gen.csv", "w");
       fprintf(f, "code_phase,prn\n");
       header = 0;
    }
    fprintf(f, "%f,%d\n", code_phase, code);

    ip = code * cos(carr_phase * 2 * M_PI);
    if (ip > 0) {
      samples[i] = 1;
    } else if (ip < 0) {
      samples[i] = -1;
    } else {
      samples[i] = 0;
    }

    carr_phase += carr_phasestep;
    code_phase += code_phasestep;

    switch (signal_type) {
    case L1CA_SIGNAL:
      if (code_phase >= L1CA_CHIPS_PER_PRN_CODE) {
        code_phase -= L1CA_CHIPS_PER_PRN_CODE;
      }
      break;
    case L2C_SIGNAL:
      if (code_phase >= 2 * L2C_CM_CHIPS_PER_PRN_CODE) {
        code_phase -= 2 * L2C_CM_CHIPS_PER_PRN_CODE;
      }
      break;
    }
  }
  signal.samples = samples;
  signal.size = samples_num;

  return signal;
}

static s8* get_prn_code(const u8 *code, size_t code_size,
                      size_t chips_per_prn)
{
  s8* prn_code;
  u32 i = 0;
  u32 j;
  u32 k;
  u8 packed;

  prn_code = malloc(chips_per_prn);
  if (NULL == prn_code) {
    return NULL;
  }
  j = 0;
  do {
    packed = code[j++];
    for (k = 0; k < 8; k++) {
      prn_code[i++] = (packed & 0x80) ? 1 : -1;
      packed <<= 1;
      if (i >= chips_per_prn) {
        break;
      }
    }
  } while ((i < chips_per_prn) && (j < code_size));
  return prn_code;
}

#define SAMPLING_FREQ_HZ        25e6
#define L1CA_CHIPPING_RATE_HZ   1.023e6
#define L2C_CM_CHIPPING_RATE_HZ 1.023e6
#define IF_FREQUENCY_HZ         2e6
#define CARRIER_DOPPLER_FREQ_HZ 100

START_TEST(test_l1ca_correlator)
{
  struct signal signal;
  s8* code;
  double init_code_phase = 0;
  double init_carr_phase = 0;
  double I_E;
  double Q_E;
  double I_P;
  double Q_P;
  double I_L;
  double Q_L;
  u32 num_samples;

  code = get_prn_code(gps_l1ca_code, sizeof(gps_l1ca_code),
                      L1CA_CHIPS_PER_PRN_CODE);
  fail_if(NULL == code, "Could not allocate PRN code data");

  signal = generate_signal( L1CA_SIGNAL,            /* signal type */
                            IF_FREQUENCY_HZ,        /* intermediate frequency */
                            L1CA_CHIPPING_RATE_HZ, /* code frequency */
                            CARRIER_DOPPLER_FREQ_HZ,/* carr_doppler frequency */
                            1. / 1540, /* carrier to code scaling factor */
                            SAMPLING_FREQ_HZ,       /* sampling frequency */
                            code,                   /* PRN code data */
                            1);                     /* milliseconds to generate */

  fail_if(NULL == signal.samples, "Could not generate signal data");

  debug_start_timing();

  l1_ca_track_correlate(signal.samples, signal.size,
    code,
    L1CA_CHIPS_PER_PRN_CODE,
    &init_code_phase,
    L1CA_CHIPPING_RATE_HZ / SAMPLING_FREQ_HZ,
    &init_carr_phase,
    (IF_FREQUENCY_HZ + CARRIER_DOPPLER_FREQ_HZ) * 2.0 * M_PI / SAMPLING_FREQ_HZ,
    &I_E, &Q_E,
    &I_P, &Q_P,
    &I_L, &Q_L,
    &num_samples);

  debug_stop_timing();

  debug_dump_results(num_samples, I_E, Q_E, I_P, Q_P, I_L, Q_L);

  u32 expected_samples = SAMPLING_FREQ_HZ / 1000; /* samples per PRN code */
  fail_if(num_samples != expected_samples);

  fail_if((I_P != 0) && (fabs(I_E / I_P) > 0.5));
  fail_if((I_P != 0) && (fabs(I_L / I_P) > 0.5));

  fail_if((I_E != 0) && (fabs(Q_E / I_E) > 0.006));
  fail_if((I_P != 0) && (fabs(Q_P / I_P) > 0.005));
  fail_if((I_L != 0) && (fabs(Q_L / I_L) > 0.006));

  free(code);
  free(signal.samples);
}
END_TEST

START_TEST(test_l2c_cm_correlator)
{
  struct signal signal;
  s8* code;
  double init_code_phase = 0;
  double init_carr_phase = 0;
  double I_E;
  double Q_E;
  double I_P;
  double Q_P;
  double I_L;
  double Q_L;
  u32 num_samples;

  code = get_prn_code(gps_l2cm_code, sizeof(gps_l2cm_code),
                      L2C_CM_CHIPS_PER_PRN_CODE);
  fail_if(NULL == code, "Could not allocate L2C CM PRN code data");

  signal = generate_signal( L2C_SIGNAL,             /* signal type */
                            IF_FREQUENCY_HZ,        /* intermediate frequency */
                            L2C_CM_CHIPPING_RATE_HZ,  /* code frequency */
                            CARRIER_DOPPLER_FREQ_HZ,/* carr_doppler frequency */
                            1. / 1200, /* carrier to code scaling factor */
                            SAMPLING_FREQ_HZ,       /* sampling frequency */
                            code,                   /* PRN code data */
                            20);                    /* milliseconds to generate */

  fail_if(NULL == signal.samples, "Could not generate signal data");

  debug_start_timing();

  l2c_cm_track_correlate(signal.samples, signal.size,
    code,
    2 * L2C_CM_CHIPS_PER_PRN_CODE,
    &init_code_phase,
    L2C_CM_CHIPPING_RATE_HZ / SAMPLING_FREQ_HZ,
    &init_carr_phase,
    (IF_FREQUENCY_HZ + CARRIER_DOPPLER_FREQ_HZ) * 2.0 * M_PI / SAMPLING_FREQ_HZ,
    &I_E, &Q_E,
    &I_P, &Q_P,
    &I_L, &Q_L,
    &num_samples);

  debug_stop_timing();

  debug_dump_results(num_samples, I_E, Q_E, I_P, Q_P, I_L, Q_L);

  u32 expected_samples = 20 * SAMPLING_FREQ_HZ / 1000; /* samples per PRN code */
  fail_if(num_samples != expected_samples);

  fail_if((I_P != 0) && (fabs(I_E / I_P) > 0.6));
  fail_if((I_P != 0) && (fabs(I_L / I_P) > 0.6));

  fail_if((I_E != 0) && (fabs(Q_E / I_E) > 0.006));
  fail_if((I_P != 0) && (fabs(Q_P / I_P) > 0.005));
  fail_if((I_L != 0) && (fabs(Q_L / I_L) > 0.006));

  free(code);
  free(signal.samples);
}
END_TEST

Suite* correlator_suite(void)
{
  Suite *s = suite_create("Correlator");
  TCase *tc_core = tcase_create("Core");

  tcase_add_test(tc_core, test_l1ca_correlator);
  tcase_add_test(tc_core, test_l2c_cm_correlator);
  suite_add_tcase(s, tc_core);

  return s;
}
